#include "trackingbase.h"

TrackingBase::TrackingBase(QObject *parent) :
    QObject(parent)
{
}

TrackingBase::~TrackingBase()
{

}

void TrackingBase::updateObjectList(QList<CvRect> rectList)
{
    if(objectList.isEmpty()){
        foreach (CvRect r, rectList) {
            objectList.append(newTrackingObject(r));
        }
    }else{
        buildPredictedObjectList();
        matchingObject(rectList);
    }
}

TrackingObject TrackingBase::newTrackingObject(CvRect rect)
{
    int _id;
    bool bAccepted;
    // get new ID
    do{
        bAccepted = true;
        _id = qrand();
        foreach (TrackingObject o, objectList) {
            if(_id == o.ID()){
                bAccepted = false;
                break;
            }
        }
    }while(!bAccepted);

    // get color for new object
#define scalarEqual(s1, s2) \
    s1.val[0] == s2.val[0] && \
    s1.val[1] == s2.val[1] && \
    s1.val[2] == s2.val[2]
    CvScalar _color;
    do{
        bAccepted = true;
        _color = cvScalar(qrand()%255, qrand()%255, qrand()%255);
        foreach (TrackingObject o, objectList) {
            if(scalarEqual(o.Color(), _color)){
                bAccepted = false;
                break;
            }
        }
    }while(!bAccepted);

    return TrackingObject(_id, _color, rect);

}

void TrackingBase::buildPredictedObjectList()
{
    predictedObjectList.clear();
    foreach (TrackingObject o, objectList) {
        predictedObjectList.append(o.predictedObject());
    }
}

QList<TrackingObject> TrackingBase::ObjectList()
{
    return objectList;
}

QList<TrackingObject> TrackingBase::PredictedObjectList()
{
    return predictedObjectList;
}

void TrackingBase::matchingObject(QList<CvRect> rectList)
{
    double d_threshold = 100;
    double d_min = d_threshold + 1;
    int id;
    foreach (CvRect r, rectList) {
        foreach (TrackingObject o, predictedObjectList) {
            int d = computeDistance(r, o.Rect());
            if(d < d_min){
                d_min = d;
                id = o.ID();
            }
        }
        if(d_min < d_threshold)
        {
            updateObject(id, r);
        }else
        {
            objectList.append(newTrackingObject(r));
        }
    }
}

double TrackingBase::computeDistance(CvRect rect1, CvRect rect2)
{
#define pointDistace(p1, p2 ) \
    sqrt( \
    pow(p1.x - p2.x,2) + \
    pow(p1.y - p2.y,2)  \
    )

    double d_center = pointDistace(
                TrackingObject::center(rect1),
                TrackingObject::center(rect2)
                );

    double d_size = abs(
                rect1.width*rect1.height -
                rect2.width*rect2.height);

    return d_center;// + d_size/(rect1.height + rect1.width);
}

void TrackingBase::updateObject(int ID, CvRect rect)
{
    for(int i = 0; i < objectList.length(); i++)
    {
        if(objectList[i].ID() == ID)
            objectList[i].update(rect);
    }
}
